//
// Created by luo on 2022-10-13.
//
#include "../include/Armor_detect.h"

//ROI算法部分
/*
 * 识别预处理，输出摄像头获取图像和需要裁剪的位置，防止ROI越界
 * */
cv::Mat Armor_detection::roi_cut(cv::Mat frame,std::array<cv::Point2f,2> roi_points)
{
    cv::Rect rect(roi_points[0].x,roi_points[0].y,roi_points[1].x,roi_points[1].y);

    if(roi_points[0].x < 0) roi_points[0].x = 0;
    if(roi_points[0].y < 0) roi_points[0].y = 0;
    if(roi_points[0].x > WIDTH) roi_points[0].x = WIDTH;
    if(roi_points[0].y > HEIGHT) roi_points[0].y = HEIGHT;

    cv::Mat cut_frame = cv::Mat(frame,rect);
}

/*
 * 若识别到装甲板，输入装甲板四角点坐标和当前帧ROI区域左上角点坐标，输出裁剪的位置坐标(4点数组)
 * */
std::array<cv::Point2f,4> Armor_detection::roi_region(std::vector<cv::Point2f>armor_points,cv::Point2f left_up_points)
{
    auto border = 50; //设置装甲板四周的尺寸
    std::array<cv::Point2f,4> roi_points;

    int max_x,max_y,min_x,min_y;
    max_x = MAX(MAX(armor_points[0].x,armor_points[1].x),
                (MAX(armor_points[2].x,armor_points[3].x)
                +border + left_up_points.x) );
    min_x = MIN(MIN(armor_points[0].x,armor_points[1].x),
                (MIN(armor_points[2].x,armor_points[3].x)
                -border + left_up_points.x) );

    max_y = MAX( MAX(armor_points[0].y,armor_points[1].y),
                (MAX(armor_points[2].y,armor_points[3].y)
                +border + left_up_points.y) );
    min_y = MIN(MIN(armor_points[0].y,armor_points[1].y),
                (MIN(armor_points[2].y,armor_points[3].y)
                -border + left_up_points.y) );

    roi_points[0].x = min_x; roi_points[0].y = min_y; //左上角
    roi_points[1].x = min_x; roi_points[1].y = max_y; //左下角
    roi_points[2].x = max_x; roi_points[2].y = max_y; //右下角
    roi_points[3].x = max_x; roi_points[3].y = min_y; //右上角

    return roi_points;
}

/*
 * 丢失装甲板
    1、敌方车辆死亡
    2、敌方车辆移速较快、开启小陀螺等
    3、看不到敌方车辆
    4、我方车辆云台移动较快
 * 处理方法
    1、调整ROI区域位置
    2、扩大ROI区域
    3、ROI区域设置为原图像大小
 * */

//1、扩大ROI区域位置
//还可以根据云台状态和对方移动状态进行调整
std::array<cv::Point2f,4> Armor_detection::roi_move_enlarge(std::array<cv::Point2f,4> roi_points,vector<int> last_scale,
                                   vector<int> now_scale)
{
    auto x_border = 50;
    auto y_border = 40;

    std::array<cv::Point2f,4> new_points;

    float max_x,min_x,max_y,min_y;
    max_x = roi_points[2].x + x_border; //右
    min_x = roi_points[0].x - x_border; //左
    max_y = roi_points[2].y + y_border; //下
    min_y = roi_points[0].y - y_border; //上

    //调整数据
    if (max_x >= WIDTH) max_x = WIDTH;
    if (min_x <= 0) min_x = 0;
    if (max_y >= HEIGHT) max_y = HEIGHT;
    if (min_y <= 0) min_y = 0;

    new_points[0].x = min_x; new_points[0].y = min_y; //左上角
    new_points[1].x = min_x; new_points[1].y = max_y; //左下角
    new_points[2].x = max_x; new_points[2].y = max_y; //右下角
    new_points[3].x = max_x; new_points[3].y = min_y; //右上角

    return new_points;
}

std::array<cv::Point2f, 4> Armor_detection::roi_origin()
{
    std::array<cv::Point2f, 4> new_points;
    float max_x = WIDTH;
    float min_x = 1;
    float max_y = HEIGHT;
    float min_y = 1;

    new_points[0].x = min_x; new_points[0].y = min_y; //左上
    new_points[1].x = max_x; new_points[1].y = min_y; //右上
    new_points[2].x = max_x; new_points[2].y = max_y; //右下
    new_points[3].x = min_x; new_points[3].y = max_y; //左下
    return new_points;
}

//2、roi设置为原图像大小(多次扩大仍找不到装甲板时)

//装甲板识别算法
/*
int value;
static void On_change(int,void*); //滑动条回调函数
cv::Mat subtract_b,subtract_g,channelop_img,threshold_img;
 */

/*
 * 一、预处理
 * */

//对初始图片二值化处理，将三通道变成单通道图像,将红色或蓝色灯条分离
/*
 * 法1 (需要调低相机曝光，减少杂光影响，减少预处理复杂度)
 * 1、通道分离相减
 * 2、二值化并取交集
 * 3、形态学操作
 * 4、与灰度(亮度)二值化图取交集
 *
 * 法2
 * 将原始图片转换到HSV空间，解耦颜色和亮度，较优，但有额外性能消耗
 * 因为颜色空间转换需要遍历图像中全部像素点
 * */
cv::Mat Armor_detection::Red_base(cv::Mat &frame)
{
    cv::Mat img = frame.clone();
    std::vector<cv::Mat> splited;
    cv::split(img, splited);

    cv::Mat out,out1,out2;
    subtract(splited[2], splited[0], out1);
    subtract(splited[2], splited[1], out2);
    bitwise_and(out1,out2,out);
    cv::threshold(out, out, 60, 255, cv::THRESH_BINARY);
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5), cv::Point(-1, -1));  // ��Բ������
    morphologyEx(out, out, cv::MORPH_DILATE, kernel);

    splited.clear();
    return out;

//    cv:: Mat frame_clone = frame.clone();
//    cv::Mat midImage,out;
//    std::string Owecolor = "blue";
//    //this->ROI = frame.clone();
//
//    Mat src=frame.clone();
//    cv::cvtColor(src,src,COLOR_BGR2GRAY);
//    //imshow("COLOR_BGR2GRAY", src);
//    std::vector<cv::Mat> imgChannels;
//    split(frame_clone, imgChannels);
//    // for(size_t i=0;i<frame.rows*frame.cols;i++){
//    // 	float *data=(float *)frame.data;
//    // 	imgChannels[0].data[i]*=0.15;
//    // }
//    for(size_t i=0;i<frame.rows*frame.cols;i++){
//        float *data=(float *)frame.data;
//        imgChannels[2].data[i]*=0.55;
//    }
//    Mat element1 = getStructuringElement(MORPH_RECT, Size(10, 10));//设置内核1
//    Mat element2 = getStructuringElement(MORPH_RECT, Size(15, 15));//设置内核2
//    Mat element3 = getStructuringElement(MORPH_RECT, Size(7, 7));//设置内核3
//    Mat element4 = getStructuringElement(MORPH_RECT, Size(3, 3));//设置内核4
//    Mat element5 = getStructuringElement(MORPH_RECT, Size(25, 25));//设置内核5
//    if (Owecolor == "red") {
//        //printf("Owecolor red");
//        cv::subtract(imgChannels[0], imgChannels[2], midImage);
//        //morphologyEx(this->midImage, this->midImage, MORPH_OPEN, element1);
//        //cv::subtract(imgChannels[0], imgChannels[1], this->midImage);//����ͨ�����
//        //cv::erode(this->midImage, this->midImage, 5);
//        imshow("midImage1", midImage);
//        morphologyEx(midImage, midImage, MORPH_CLOSE, element1);
//        cv::threshold(midImage, midImage, 30, 255, cv::THRESH_BINARY);
//        imshow("midImage1", midImage);
//        bitwise_not(midImage,midImage);
//        floodFill(midImage, Point(0, 0), Scalar(0));
//        imshow("midImage2", midImage);
//        bitwise_and(src,midImage,out);
//        //imshow("out:",out);
//        //morphologyEx(this->midImage, this->midImage, MORPH_OPEN, element1);  //������
//        //dilate(this->out, this->out, element);
//        morphologyEx(out, out, MORPH_CLOSE, element4);
//        //imshow("midImage out", out);
//    }
//    else  //Owecolor blue
//    {
//        //printf("Owecolor blue");
//        cv::subtract(imgChannels[2], imgChannels[0], midImage);
//        //morphologyEx(this->midImage, this->midImage, MORPH_CLOSE, element1);
//        //imshow("midImage1", midImage);
//        cv::erode(midImage, midImage, 5);
//        cv::threshold(midImage, midImage, 80, 255, cv::THRESH_BINARY);
//        //dilate(this->midImage, this->midImage, element4);
//        imshow("midImage2", midImage);
//        //morphologyEx(this->midImage, this->midImage, MORPH_OPEN, element1);  //������
//        int structElementSize = 4;
//        cv::Mat element = getStructuringElement(MORPH_RECT, Size(2 * structElementSize + 1, 2 * structElementSize + 1), Point(structElementSize, structElementSize));
//        //bitwise_not(midImage,midImage);
//        //floodFill(this->midImage, Point(0, 0), Scalar(0));
//        dilate(midImage, out, element);            //
//        //imshow("midImage out", midImage);
//        //bitwise_and(src,this->midImage,out);
//        //dilate(this->out, this->out, element4);
//        //imshow(" out", midImage);
//    }
    return out;
}

cv::Mat Armor_detection::Red_bin_operate(cv::Mat &frame) //红色灯条二值化
{
    cv::Mat imput_img = frame.clone(); //克隆一份副本，防止原图改变，非引用
    std::vector<cv::Mat> splited_; //存放分离通道后的单通道图的数组
    cv::split(imput_img,splited_); //分离三通道 B G R 0 1 2

    cv::Mat gray_threimg,gray_filter_th_9;
    cv::cvtColor(imput_img,gray_threimg,COLOR_BGR2GRAY);
    //cv::medianBlur(gray_threimg,gray_filter_th_9,9);
    cv::threshold(gray_threimg,gray_filter_th_9,68,this->max_threashold,cv::THRESH_BINARY);//x阈值的二值化
//    cv::namedWindow("gray_filter_th_9",WINDOW_NORMAL);
//    cv::resizeWindow("gray_filter_th_9",WINDOW_WIDTH,WINDOW_HEIGHT);
//    cv::imshow("gray_filter_th_9",gray_filter_th_9);

    /*
     * 识别红色物体可以直接用r通道-b通道。由于在低曝光下只有灯条是有颜色的，两通道相减后，
     * 其他区域的部分会因为r和b的值差不多而被减去，而蓝色灯条部分由于r通道比b通道的值小，
     * 相减后就归0了，也就是剩下的灰度图只留下了红色灯条。
     * */
    //1、通道分离相减
    cv::Mat subtract_b,subtract_g,channelop_img,threshold_img,init_img;
    cv::subtract(splited_[2],splited_[0],subtract_b); //红色通道-蓝色通道
    cv::subtract(splited_[2],splited_[1],subtract_g); //红色通道-绿色通道
    cv::bitwise_and(subtract_b,subtract_g,channelop_img);//降低RGB亮度与颜色信息耦合

//    cv::namedWindow("subtract_b",WINDOW_NORMAL);
//    cv::resizeWindow("subtract_b",WINDOW_WIDTH,WINDOW_HEIGHT);
//    cv::imshow("subtract_b",subtract_b);
//
//    cv::namedWindow("channelop_img",WINDOW_NORMAL);
//    cv::resizeWindow("channelop_img",WINDOW_WIDTH,WINDOW_HEIGHT);
//    cv::imshow("channelop_img",channelop_img);

    //2、二值化并取交集 ( 怎么采取 阈值自适应 进行优化)
    //value = 71; //71 73 使用矩形内核 7*7 开运算较合适 单图
    //value = 81; //交集图
    //cv::namedWindow("threshold_change");
    //cv::resizeWindow("threshold_change",700,700);
    //cv::createTrackbar("threshold","threshold_change",&value,255,On_change,0);

    cv::threshold(channelop_img,threshold_img,this->min_threashold,this->max_threashold,cv::THRESH_BINARY);//x阈值的二值化
    //cv::imshow("threshold_change",threshold_img);

    //3、形态学操作
    //3-1使用非线性中值滤波去除较小噪点(二值图像中的小斑点)
    //cv::Mat filterResult_9;
    //cv::medianBlur(threshold_img,filterResult_9,9);//虽然能较好的去除大部分小连通域噪点，但开销大大增加
    //cv::imshow("filter",filterResult_9);

    //3-2形态学操作，按最优解选择方案
    cv::Size ksize_3 = Size(3,3);
    cv::Size ksize_5 = Size(5,5);
    cv::Size ksize_7 = Size(7,7);
    cv::Size ksize_10 = Size(10,10);
    //cv::Mat kernel_e = cv::getStructuringElement(cv::MORPH_ELLIPSE,ksize_5,cv::Point(-1,-1));
    cv::Mat kernel_r = cv::getStructuringElement(cv::MORPH_RECT,ksize_10,cv::Point(-1,-1));

    //cv::morphologyEx(threshold_img,init_img,cv::MORPH_DILATE,kernel_r); //膨胀,”变胖”,使得高亮区域或者白色区域变大
    //图像的连通域是指图像中具有相同像素值并且位置相邻的像素组成的区域

    //配合蓝－红/蓝-绿 二值化并取交集图
    cv::morphologyEx(threshold_img,init_img,cv::MORPH_CLOSE,kernel_r);//闭运算 防止灯条在直接膨胀后的面积扩张
//    cv::namedWindow("init_img",WINDOW_NORMAL);
//    cv::resizeWindow("init_img",WINDOW_WIDTH,WINDOW_HEIGHT);
//    cv::imshow("init_img",init_img);

    //配合蓝－红的单图
    //cv::morphologyEx(threshold_img,init_img,cv::MORPH_OPEN,kernel_r); //开运算,去除了较小的连通域 保留了较大的连通域。

    //4、与灰度(亮度)二值化图取交集
    cv::bitwise_and(gray_filter_th_9,init_img,init_img);
    //cv::waitKey(0);
    splited_.clear(); //释放vector容器内存
    return init_img;
}

/*
static void On_change(int,void*)
{
    cv::threshold(channelop_img,threshold_img,value,255,cv::THRESH_BINARY);//x阈值的二值化

    cv::imshow("threshold_change",threshold_img);

    cv::Mat grayResult_9;
    cv::medianBlur(threshold_img,grayResult_9,9);
    cv::imshow("filter",grayResult_9);

    //3、形态学操作
    cv::Size ksize_3 = Size(3,3);
    cv::Size ksize_5 = Size(5,5);
    cv::Size ksize_7 = Size(7,7);
    cv::Size ksize_10 = Size(10,10);

    //cv::Mat kernel_e = cv::getStructuringElement(cv::MORPH_ELLIPSE,ksize_5,cv::Point(-1,-1));
    cv::Mat kernel_r = cv::getStructuringElement(cv::MORPH_RECT,ksize_10,cv::Point(-1,-1));

    //cv::morphologyEx(output_img,output_img,cv::MORPH_DILATE,kernel_e); //膨胀,”变胖”,使得高亮区域或者白色区域变大
    //cv::dilate(threshold_img,threshold_img,kernel_r);
    //图像的连通域是指图像中具有相同像素值并且位置相邻的像素组成的区域

    cv::morphologyEx(grayResult_9,grayResult_9,cv::MORPH_CLOSE,kernel_r);//闭运算 先膨胀后腐蚀,防止灯条在直接膨胀后的面积扩张

    //cv::morphologyEx(threshold_img,threshold_img,cv::MORPH_OPEN,kernel_r); //开运算,先腐蚀后膨胀，去除了较小的连通域 保留了较大的连通域。
//    cv::namedWindow("red_result",WINDOW_NORMAL);
//    cv::imshow("red_result",output_img);
    cv::namedWindow("red_result",WINDOW_NORMAL);
    cv::resizeWindow("red_result",700,700);
    cv::imshow("red_result",grayResult_9);
}
 */

/*
 * 二、寻找轮廓
 * 寻找二值图的白色区域，便于后续筛选
 * */
std::vector<std::vector<cv::Point2i>>Armor_detection::Find_contour(cv::Mat thre_img)
{
    //这里为什么要用容器嵌套？
    //图中可能不止一个二值封闭区域，因此每个封闭区域为最外层vector，每个封闭区域的二维轮廓坐标点为内层vector
    //cv::Mat thre_conturs_img = img.clone();
    std::vector<std::vector<cv::Point2i>> thre_contours; //存放二值图区域的白色轮廓
    std::vector<Vec4i> thre_hierarchy; //存放二值图轮廓结构变量
    /*
     * hierarchy：轮廓结构关系描述向量，若只需检测图像轮廓则不需此变量，否则会造成内存浪费
     * RETR_LIST：提取全部轮廓，放在容器中
     * CHAIN_APPROX_NONE：获取每个轮廓的每个像素，相邻两个点的像素位置相差1
     * CHAIN_APPROX_SIMPLE：压缩水平、垂直、对角线方向元素，保留方向上的重要坐标.如矩形轮廓四个点
     * */
    //cv::findContours(thre_img,thre_contours,thre_hierarchy,cv::RETR_LIST,cv::CHAIN_APPROX_SIMPLE);
    cv::findContours(thre_img,thre_contours,cv::RETR_LIST,cv::CHAIN_APPROX_SIMPLE);
    for(int t = 0; t < thre_contours.size(); t++)
    {
        //第三个参数为绘制的轮廓数目
        //图像要为三通道才能显示轮廓，在原图上面绘制!!!!!!!
        //std::cout<<"thre_contours:"<<thre_contours[t]<<std::endl;
        cv::drawContours(raw_img,thre_contours,t,Scalar(255,255,0),2,8);
    }
//    for(int i = 0; i < thre_hierarchy.size(); i++)
//    {
//        std::cout<<thre_hierarchy[i]<<std::endl;
//    }

    return thre_contours;
}

/*
 * 三、筛选灯条
 * 对步骤二找到的轮廓进行筛选，筛选出是装甲板上的灯条
 *  筛选条件：
        灯条的长宽比(位于某个区间内)
        灯条的颜色(红或蓝)
 * */
/*
 * 关于RotatedRect旋转矩形
 *  RotatedRect类是OpenCV的基础类，用于创建旋转矩形，下面是它的构造函数，包含旋转中心点、尺寸大小和旋转角度。
 *  RotatedRect(const Point2f& center, const Size2f& size, float angle);
    center	旋转矩形的质心
    size	旋转矩形的宽度和高度
    angle	顺时针方向的旋转角度。当角度为0°，90°，180°，270°等时，矩形变为右上方的矩形

 *  RotatedRect(const Point2f& point1, const Point2f& point2, const Point2f& point3);
 *  通过这个构造函数也可以创建旋转矩形，只需要给出三个点，但是要指定三个点的顺序。

    矩形的Width与Height定义：矩形坐标系中X轴逆时针旋转，碰到的第一条边为Width，另一条边为Height，
    其中矩形坐标系的原点为图像坐标系下矩形四个顶点中y值最大的点

    顶点p[0]的位置可以这样理解：
    ⓐ 如果没有边与坐标轴平行，则Y坐标最大的点为p[0]点，如矩形(2)(3)(4)；
    ⓑ 如果有边与坐标轴平行，则有两个Y坐标最大的点。此时，左侧的点为p[0]点。如矩形(1)。
    ** p[0] - p[3] 按顺时针方向以此排列 **
 * */
//bool Armor::select_lamp_(cv::Mat raw_img,std::vector<std::vector<cv::Point2i>>thre_contours)
std::vector<cv::RotatedRect> Armor_detection::select_lamp(cv::Mat raw_img,std::vector<std::vector<cv::Point2i>>thre_contours)
{
    std::vector<cv::RotatedRect> func_lamp_list; //存储合理灯条的容器
    for(size_t i = 0; i < thre_contours.size(); i++)
    {
        // RotatedRect cv::minAreaRect(InputArray points)
        // 求取带角度最小面积包围矩形，points是点的集合，如轮廓
        cv::RotatedRect r_rect = cv::minAreaRect(thre_contours[i]); //找出最小旋转矩形
        //cv::RotatedRect r_rect = cv::fitEllipse(thre_contours[i]); //找出最小椭圆

        double area = contourArea(thre_contours[i]); //计算图像轮廓的面积
        if(area<15 || area>5000) continue; //略过不合理灯条

        //*********************寻找矩形的长、宽**************************
        double r_rect_len; //矩形长
        double r_rect_wid; //矩形宽

        if(r_rect.size.width > r_rect.size.height)
        {
            r_rect_len = r_rect.size.width; //长的那边赋值为长
            r_rect_wid = r_rect.size.height; //短的那边赋值为宽
        }
        else
        {
            r_rect_len = r_rect.size.height; //长的那边赋值为长
            r_rect_wid = r_rect.size.width; //短的那边赋值为宽
        }

        /*-------------------------------------------------*/
        //在图像坐标系中，原点在左上角，向右为正向x轴，向下为正向y轴，因此y代表行，x代表列
        cv::Vec3b v_3b = raw_img.at<cv::Vec3b>((float)(r_rect.center.y),(float)(r_rect.center.x));//找到旋转矩形中心在原图像素的位置
        double len_wid_ratio = r_rect_len / r_rect_wid; //得到灯条的长宽之比

        bool if_close_dis = (len_wid_ratio >= 1.8) && (len_wid_ratio < 15); //近距离的灯条长宽比
        bool if_lamp_notthwartwise = !(r_rect.angle > -45 && r_rect.size.width == r_rect_len) ||(r_rect.angle < -45 && r_rect.size.width == r_rect_wid); //灯条不能横着
        //bool if_mid_color = (v_3b[0] > 160) || (v_3b[2] > 160); //判断中心点颜色,r,b值较低时
        //bool if_area = area < 400; //判断距离是否较远??
        bool if_area = (area>100)&&(area<280); //判断距离是否较远??
        bool if_far_dis = (len_wid_ratio > 1.08) && (len_wid_ratio < 3.1);//远距离时灯条的长宽比。远距 离时灯条较小，在二值化后的膨胀会导致灯条偏胖。

//        std::cout << i << ":" << std::endl;
//        std::cout << "len_wid_ratio:"<<len_wid_ratio << std::endl;
//        std::cout << "area:" << area << std::endl;
//        std::cout << "if_close_dis:" << if_close_dis << std::endl;
//        std::cout << "if_area:" << if_area << std::endl;
//        std::cout << "if_far_dis:" << if_far_dis << std::endl;

        //if((if_close_dis || (if_area && if_far_dis)) && if_mid_color)
        if(if_close_dis || (if_area && if_far_dis) && if_lamp_notthwartwise )
        {
            func_lamp_list.push_back((r_rect));//放入此矩形到灯条容器内
            //this->lamp_list.push_back((r_rect));//放入此矩形到灯条容器内
            //std::cout << "lamp center (x,y)：" << r_rect.center << std::endl;

            cv::Point2f vertices[4]; //存放外接矩形四个点坐标
            r_rect.points(vertices); //返回外接矩形四个顶点
            for(int i = 0; i < 4; i++)
            {
                //框出灯条轮廓
                cv::line(raw_img,vertices[i],vertices[(i+1)%4],cv::Scalar(255,0,0),2);
            }
        }
    }
    thre_contours.clear();
//    if (this->lamp_list.size() < 2) //少于两个灯条则表明当前找不到装甲板
//    {
//        return false;
//    }
    //this->lamp_list =  func_lamp_list;
    //return true;
    return func_lamp_list;
}

/*
 * 四、筛选装甲板
 * 筛选出哪两个灯条能组成装甲板,以及发现多个装甲板应选择击打哪一个
 * 装甲板：两灯条组成的四边形
    判断条件：
        两灯条角度(“非 八字”)
        四边形的长宽比(灯条间距以及灯条平均长度在某个区间内)
        两灯条中点连成的线段与水平的角度小于某个阈值
 * 区别大小装甲板：
     四边形的长宽比(灯条间距以及灯条平均长度在某个区间内)
     两灯条的大小差距
 * */

/**
  *  灯条根据x轴坐标在数组中升序排列
  * @param a     低位地址   ，     b       高位地址
  * @return bool 升序格式
   */
bool campare_lamp(RotatedRect & lamp1,RotatedRect & lamp2)
//static inline bool campare_lamp(RotatedRect & lamp1,RotatedRect & lamp2)
{
    return lamp1.center.x < lamp2.center.x;
}

/*两点距离
	  @param p1 点1
	  @param p2 点2
	  @return 距离*/
float pointDistance(const cv::Point2f &p1, const cv::Point2f &p2) {
    return std::sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
}

double Point_angle(const cv::Point2f point1,const cv::Point2f point2){
    double angle = atan2(abs(point1.y - point2.y), abs(point1.x - point2.x)) * 180 / CV_PI;
    return angle;
}

float Make_angle(const cv::Point2f & m_Points1,const cv::Point2f & m_Points2) {
    float coordinate_x;
    float coordinate_y;
    cv::Point2f c_coordinate;
    coordinate_x = m_Points2.x - m_Points1.x;
    coordinate_y = m_Points2.y - m_Points1.y;
    //第一象限
    if (coordinate_x > 0 && coordinate_y < 0) {
        c_coordinate.x = coordinate_x;
        c_coordinate.y = -coordinate_y;
    }
        //第二象限
    else if (coordinate_x < 0 && coordinate_y < 0)
    {
        c_coordinate.x = coordinate_x;
        c_coordinate.y = -coordinate_y;
    }
        //第三象限
    else if (coordinate_x < 0 && coordinate_y > 0)
    {
        c_coordinate.x = coordinate_x;
        c_coordinate.y = -coordinate_y;
    }
        //第四象限
    else {
        c_coordinate.x = coordinate_x;
        c_coordinate.y = -coordinate_y;
    }
    float angle = atan2(c_coordinate.y, c_coordinate.x) * 180 / CV_PI;
    //cout << "angle 11 :" << angle << endl;
    return angle;
}

//bool Armor::select_armor_(std::vector<cv::RotatedRect> lamp_list)
std::vector<cv::Point2f> Armor_detection::select_armor(std::vector<cv::RotatedRect> lamp_list)
{

    if (this->lamp_list.size() > 1)
    {
        //*******将旋转矩形从左到右排序  very important!!*********//
        sort(lamp_list.begin(), lamp_list.end(), campare_lamp);
    }

    //std::cout<<666<<std::endl;
    std::vector<cv::Point2f> armor_;
    std::vector<std::vector<cv::Point2f>> armor_list_;

    //定义装甲板的四个点，面积，角度
    cv::Point2f point_;
    cv::Size2f areas_;
    float angle_;

    std::vector<cv::Point2f> point_list_; //存放灯条轮廓中心点的容器
    std::vector<cv::Size2f> area_list_; //存放灯条面积的容器
    std::vector<float> angle_list_; //存放灯条角度的容器


    for(size_t t = 0; t < lamp_list.size(); t++)
    {
        //最小旋转矩形的中心点坐标
        point_ = lamp_list[t].center;
        point_list_.push_back(point_);

        //最小旋转矩形面积
        areas_ = lamp_list[t].size;
        area_list_.push_back(areas_);

        //最小旋转矩形角度
        angle_ = lamp_list[t].angle;
        angle_list_.push_back(angle_);
//        std::cout<<"point_:"<<lamp_list[t].center<<std::endl;
//        std::cout<<"areas_:"<<lamp_list[t].size<<std::endl;
//        std::cout<<"angle_:"<<lamp_list[t].angle<<std::endl;
    }

    //*******以前的for循环是错误的已经更改非常重要*******//
//    for(size_t i = 0; angle_list_.size()-i-1; i++)//通过灯条的角度对比筛选装甲板
//    {
//        for(size_t j = 1; angle_list_.size()-j-1; j++)
//        {//这里的循环判断条件为什么要用 angle_list_.size()-i-1 ?
    lamp1 _Lamp1;
    lamp2 _Lamp2;
    for (size_t i = 0; i < lamp_list.size() - 1; i++)
    {
        for (size_t j = i + 1; j < lamp_list.size(); j++)
        {
            if(i == j) continue; //同灯条不比较

            float a_dif,px_dif,py_dif,p_dis,ag_dif;

            //两个最小旋转矩形的面积差
            a_dif = float(abs(area_list_[i].height*area_list_[i].width -
                    area_list_[j].height*area_list_[j].width) );

            //两个最小旋转矩形X,Y轴的差
            px_dif = abs(float(point_list_[i].x) - float(point_list_[j].x));
            py_dif = abs(float(point_list_[i].y)-float(point_list_[j].y));
            //两个最小旋转矩形水平角度
            float ban_angle = atan2(py_dif, px_dif)*180/PI;

            //两个最小旋转矩形的距离
            p_dis = sqrt(pow(px_dif,2) + pow(py_dif,2));

            //两个最小旋转矩形角度差
            ag_dif = abs(float(angle_list_[i]) - float(angle_list_[j]));

            //设定两个最小旋转矩形的平均长度
            double max_lamp_i_len = MAX(area_list_[i].width,area_list_[i].height);
            double max_lamp_j_len = MAX(area_list_[j].width,area_list_[j].height);
            double average_len = (max_lamp_i_len + max_lamp_j_len) / 2;
            float lw_ratio = p_dis / average_len; // 灯条间距/灯条高度

            //设定两个最小旋转矩形的平均角度
            float angle_i = abs(angle_list_[i]);
            float angle_j = abs(angle_list_[j]);
            if(angle_i > 70) angle_i = 90 - angle_i;
            if(angle_j > 70) angle_j = 90 - angle_j;
            double average_angle = (angle_i + angle_j)/2;

            //筛选较小的灯条 两个灯条面积差 < 两个灯条的平均面积和的二分之一
            bool if_area = a_dif < float((area_list_[i].height*area_list_[i].width +
                                             area_list_[j].height*area_list_[j].width) /2 );

            //bool if_y_dif = py_dif < 20; //两灯条纵向间距
            bool if_ev_ag_dif = abs(average_angle - atan((py_dif+0.1)/px_dif) *180.0f/PI) < 23; //两个灯条的平均角度和装甲板的角度差 < 22

            /*
             * 小装甲板 灯光间距比灯光高度为 14:6 识别为 2.0+
             * 大装甲板 灯光间距比灯光高度为 23:6 识别为 6.5+
             * */
            //bool if_small_dis = (1.8 < lw_ratio) && (lw_ratio< 3.0); // 小装甲板的灯条间距/灯条高度
            //bool if_big_dis = (3.7 < lw_ratio) && (lw_ratio< 5.5); // 大装甲板的灯条间距/灯条高度
            bool if_small_dis = (1.4 < lw_ratio) && (lw_ratio< 4.8); // 小装甲板的灯条间距/灯条高度
            bool if_big_dis = (5 < lw_ratio) && (lw_ratio< 7); // 大装甲板的灯条间距/灯条高度
            if (1.4 > lw_ratio || lw_ratio > 5)
            {
                continue;
            }

            //bool if_s_lam = p_dis >= 2; //是否为同一灯条
            //bool if_bubing_dif = (1.6 < p_dis / average_len) && (p_dis / average_len< 5.5); // 步兵视角装甲板的灯条间距/灯条高度
            //bool if_ban_gg = (ban_angle<10)||(ban_angle>80);
            bool if_ban_ag = (ban_angle<20)||(ban_angle>0);
            bool if_ag_dif = ag_dif < 10 || ag_dif > 88; //两个灯条角度差小于30或大于75

//            std::cout <<"if_area:"<<if_area <<std::endl<<"if_ev_ag_dif:"<< if_ev_ag_dif<<std::endl
//            << " if_bubing_dif:"<<if_bubing_dif <<std::endl<< " if_ag_dif:"<<if_ag_dif<<std::endl;
//            std::cout << "Left lamp angle：" << angle_i << std::endl;
//            std::cout << "Right lamp angle：" << angle_j << std::endl;
//            std::cout << "lamp angle dif:" << ag_dif << std::endl;
//            std::cout << "average angle：" << average_angle << std::endl;
//            std::cout << "lamp area dif：" << a_dif << std::endl;
//            std::cout << "average area:" << float(area_list_[i].height * area_list_[i].width + area_list_[j].height * area_list_[j].width) / 2 << std::endl;
//            std::cout << "deck angle：" << atan((py_dif + 0.1) / px_dif)*180.0f / PI << std::endl;
//            std::cout << "lamp_dis/lamp_hei:" << p_dis / average_len << std::endl << std::endl;

            if(if_area && if_ev_ag_dif && (if_small_dis || if_big_dis) && if_ban_ag && if_ag_dif)
            {
                //定义左右两个灯条的旋转矩形
//                cv::RotatedRect lamp_l,lamp_r; //左右灯条对象
//                lamp_l.center = point_list_[i];//中心点坐标
//                lamp_l.size = area_list_[i];//灯条面积
//                lamp_l.angle = angle_list_[i];//灯条角度
//                lamp_r.center = point_list_[j];//中心点坐标
//                lamp_r.size = area_list_[j];//灯条面积
//                lamp_r.angle = angle_list_[j];//灯条角度

                //_Lamp1对象内旋转矩形lamp1，其包含center,size,angle三个属性，points()矩形四个顶点成员
                _Lamp1.lamp1.center = point_list_[i]; //坐标
                _Lamp1.lamp1.size = area_list_[i];	   //面积
                _Lamp1.lamp1.angle = angle_list_[i];   //角度
                _Lamp2.lamp2.center = point_list_[j]; //坐标
                _Lamp2.lamp2.size = area_list_[j];	   //面积
                _Lamp2.lamp2.angle = angle_list_[j];   //角度

                //转换为装甲板四个顶点
                //cv::Point2f vertices_l[4],vertices_r[4];
                cv::Point2f vertices0[4], vertices1[4]; //转换成4个点
                _Lamp1.lamp1.points(vertices0);
                _Lamp2.lamp2.points(vertices1);

                for(size_t x = 0; x <4; x++)
                {
                    circle(this->raw_img, vertices0[x], 3, Scalar(255, 255, 0), 5);
                    circle(this->raw_img, vertices1[x], 3, Scalar(255, 255, 0), 5);
                }

                //转换为装甲板四个点
                //中途没有筛选出四个点，而是直接相连两灯条中心处*****
//                if (vertices_r[1].y < vertices_r[3].y)
//                {   point_.x = (vertices_r[1].x + vertices_r[2].x) / 2;
//                    point_.y = (vertices_r[1].y + vertices_r[2].y) / 2;
//                    armor_.push_back(point_);
//
//                    point_.x = (vertices_r[3].x + vertices_r[0].x) / 2;
//                    point_.y = (vertices_r[3].y + vertices_r[0].y) / 2;
//                    armor_.push_back(point_); }
//                else {
//                    point_.x = (vertices_r[3].x + vertices_r[2].x) / 2;
//                    point_.y = (vertices_r[3].y + vertices_r[2].y) / 2;
//                    armor_.push_back(point_);
//
//                    point_.x = (vertices_r[1].x + vertices_r[0].x) / 2;
//                    point_.y = (vertices_r[1].y + vertices_r[0].y) / 2;
//                    armor_.push_back(point_);
//                }
//
//                if (vertices_l[1].y < vertices_l[3].y) {
//                    point_.x = (vertices_l[3].x + vertices_l[0].x) / 2;
//                    point_.y = (vertices_l[3].y + vertices_l[0].y) / 2;
//                    armor_.push_back(point_);
//
//                    point_.x = (vertices_l[1].x + vertices_l[2].x) / 2;
//                    point_.y = (vertices_l[1].y + vertices_l[2].y) / 2;
//                    armor_.push_back(point_); }
//                else {
//                    point_.x = (vertices_l[1].x + vertices_l[0].x) / 2;
//                    point_.y = (vertices_l[1].y + vertices_l[0].y) / 2;
//                    armor_.push_back(point_);
//
//                    point_.x = (vertices_l[3].x + vertices_l[2].x) / 2;
//                    point_.y = (vertices_l[3].y + vertices_l[2].y) / 2;
//                    armor_.push_back(point_);
//                }
                //判定旋转矩形的四个点哪个是起点，按逆时针顺序0 1 2 3 排序赋值，并抽出左右灯条四点
                if (vertices0[0].y < vertices0[2].y)
                {
                    _Lamp1.lamp1_point1.x = (vertices0[1].x + vertices0[0].x) / 2;
                    _Lamp1.lamp1_point1.y = (vertices0[1].y + vertices0[0].y) / 2;
                    _Lamp1.lamp1_point2.x = (vertices0[3].x + vertices0[2].x) / 2;
                    _Lamp1.lamp1_point2.y = (vertices0[3].y + vertices0[2].y) / 2;
                    _Lamp1.lamp1_width = pointDistance(vertices0[1], vertices0[0]);
                }
                else if (vertices0[0].y > vertices0[2].y)
                {
                    _Lamp1.lamp1_point1.x = (vertices0[1].x + vertices0[2].x) / 2;
                    _Lamp1.lamp1_point1.y = (vertices0[1].y + vertices0[2].y) / 2;
                    _Lamp1.lamp1_point2.x = (vertices0[3].x + vertices0[0].x) / 2;
                    _Lamp1.lamp1_point2.y = (vertices0[3].y + vertices0[0].y) / 2;
                    _Lamp1.lamp1_width = pointDistance(vertices0[2], vertices0[1]);
                }
                else
                {
                    _Lamp1.lamp1_point1.x = (vertices0[0].x + vertices0[1].x) / 2;
                    _Lamp1.lamp1_point1.y = (vertices0[0].y + vertices0[1].y) / 2;
                    _Lamp1.lamp1_point2.x = (vertices0[3].x + vertices0[2].x) / 2;
                    _Lamp1.lamp1_point2.y = (vertices0[3].y + vertices0[2].y) / 2;
                    _Lamp1.lamp1_width = pointDistance(vertices0[0], vertices0[1]);
                }

                if (vertices1[0].y < vertices1[2].y)
                {
                    _Lamp2.lamp2_point1.x = (vertices1[1].x + vertices1[0].x) / 2;
                    _Lamp2.lamp2_point1.y = (vertices1[1].y + vertices1[0].y) / 2;
                    _Lamp2.lamp2_point2.x = (vertices1[3].x + vertices1[2].x) / 2;
                    _Lamp2.lamp2_point2.y = (vertices1[3].y + vertices1[2].y) / 2;
                    _Lamp2.lamp2_width = pointDistance(vertices1[1], vertices1[0]);
                }
                else if (vertices1[0].y > vertices1[2].y)
                {
                    _Lamp2.lamp2_point1.x = (vertices1[1].x + vertices1[2].x) / 2;
                    _Lamp2.lamp2_point1.y = (vertices1[1].y + vertices1[2].y) / 2;
                    _Lamp2.lamp2_point2.x = (vertices1[3].x + vertices1[0].x) / 2;
                    _Lamp2.lamp2_point2.y = (vertices1[3].y + vertices1[0].y) / 2;
                    _Lamp2.lamp2_width = pointDistance(vertices1[1], vertices1[2]);
                }
                else
                {
                    _Lamp2.lamp2_point2.x = (vertices0[0].x + vertices0[1].x) / 2;
                    _Lamp2.lamp2_point2.y = (vertices0[0].y + vertices0[1].y) / 2;
                    _Lamp2.lamp2_point1.x = (vertices0[3].x + vertices0[2].x) / 2;
                    _Lamp2.lamp2_point1.y = (vertices0[3].y + vertices0[2].y) / 2;
                    _Lamp2.lamp2_width = pointDistance(vertices0[0], vertices0[1]);
                }
                //**********灯条的高度************//
                _Lamp1.lamp1_heigth = pointDistance(_Lamp1.lamp1_point1, _Lamp1.lamp1_point2);
                _Lamp2.lamp2_heigth = pointDistance(_Lamp2.lamp2_point1, _Lamp2.lamp2_point2);

                //**********灯条的面积************//
                _Lamp1.lamp1_area = _Lamp1.lamp1_width * _Lamp1.lamp1_heigth;
                _Lamp2.lamp2_area = _Lamp2.lamp2_heigth * _Lamp2.lamp2_width;

                vector<cv::Point2f> lamp_mid_point;
                lamp_mid_point.push_back(_Lamp1.lamp1_point1);
                lamp_mid_point.push_back(_Lamp2.lamp2_point1);
                lamp_mid_point.push_back(_Lamp2.lamp2_point2);
                lamp_mid_point.push_back(_Lamp1.lamp1_point2);
                //cv::line(this->raw_img, _Lamp1.lamp1_point1, _Lamp2.lamp2_point2, cv::Scalar(255, 255, 0), 2);
                //cv::line(this->raw_img, _Lamp1.lamp1_point2, _Lamp2.lamp2_point1, cv::Scalar(255, 255, 0), 2);
                for(auto i = 0; i < 4; i++)
                {
                    cv::line(this->raw_img, lamp_mid_point[i],lamp_mid_point[(i+1)%4],cv::Scalar(255, 255, 0), 3);
                }

                //*********灯条的高度比值************//
                float height_rate = _Lamp2.lamp2_heigth / _Lamp1.lamp1_heigth;
                if (height_rate > 1.6 || height_rate < 0.7)
                {
                    continue;
                }
                //*********两个灯条面积差************//
                float area_diff = abs(_Lamp1.lamp1_area - _Lamp2.lamp2_area);
                // tool::data(area_diff,"area_diff");
                area_diff < float((_Lamp1.lamp1_area + _Lamp1.lamp1_area) / 2);
                area_diff > min(_Lamp1.lamp1_area, _Lamp2.lamp2_area);

                //*********两个灯条面积比值************//
                float area_rate = abs(_Lamp1.lamp1_area / _Lamp2.lamp2_area);
                // tool::data(area_rate, "area_rate");
                if (area_rate > 3 || area_rate < 0.4)
                {
                    continue;
                }

                //*********算出灯条的角度 两个灯条角度差 极值15 或17************//
                double lamp1_angle = Point_angle(_Lamp1.lamp1_point1, _Lamp1.lamp1_point2);
                // tool::data(lamp1_angle, "lamp1_angle");
                _Lamp1.lamp1_angle = lamp1_angle;
                double lamp2_angle = Point_angle(_Lamp2.lamp2_point1, _Lamp2.lamp2_point2);
                _Lamp2.lamp2_angle = lamp2_angle;
                double angle_diff = abs(lamp1_angle - lamp2_angle);
                // tool::data(angle_diff, "angle_diff");
                if (angle_diff > 15)
                {
                    continue;
                }

                //********根据平行角度差筛选,atan2(y,x)相对与直角坐标系 **********// **重要
                //********添加装甲板角度 可用于仿射变换**********// **重要
                // float Rect_angle = fabs(atan2(_lamp.lamp2_point2.y - _lamp.lamp1_point2.y, _lamp.lamp2_point2.x - _lamp.lamp1_point2.x) * 180 / CV_PI);
                float _armor_angle2 = Make_angle(_Lamp1.lamp1_point2, _Lamp2.lamp2_point2);
                // tool::data(_armor_angle, "Rect_angle");
                if (25 < _armor_angle2 || _armor_angle2 < -25)
                {
                    continue;
                }

                //********灯条最低点高于灯条最高点,不符合过滤掉**********//
                //这个有问题
                if (_Lamp1.lamp1_point2.y + _Lamp1.lamp1_heigth * 1 / 5 < _Lamp2.lamp2_point1.y || _Lamp2.lamp2_point2.y < _Lamp1.lamp1_point1.y + _Lamp1.lamp1_heigth * 1 / 5)
                {
                    continue;
                }

                //********第一个灯条到第二个灯条的斜长**********//
                float pointDistance_tilt1 = pointDistance(_Lamp1.lamp1_point1, _Lamp2.lamp2_point2);
                float pointDistance_tilt2 = pointDistance(_Lamp1.lamp1_point2, _Lamp2.lamp2_point1);
                float tilt_distance_rate = pointDistance_tilt1 / pointDistance_tilt2; // dy/dx 求出斜率
                //***********斜率长度比不符合过滤掉************//
                // tool::data(tilt_distance_rate,"tilt_distance_rate");
                if (tilt_distance_rate > 1.4 || tilt_distance_rate < 0.7)
                {
                    continue;
                }
                //********第一个灯条到第二个灯条的斜长角度 **********//
                //********以前为比值现在为做差*******//(更改)
                double bevel_angle1 = fabs(Make_angle(_Lamp1.lamp1_point1, _Lamp2.lamp2_point2));
                double bevel_angle2 = Make_angle(_Lamp1.lamp1_point2, _Lamp2.lamp2_point1);
                double ratio_all = abs(bevel_angle1 / bevel_angle2);

                //armor_list_.push_back(armor_);

//                for(int n = 0; n < 4; n++)
//                {
//                    cv::line(this->raw_img,armor_[n],armor_[(n+1) % 4],cv::Scalar(0,255,0),5);
//                }
            }
        }
    }
    float area, max_area = 0;
    for (size_t k = 0; armor_list_.size() - k; k++)
    {
        area = abs((armor_list_[k][0].x - armor_list_[k][2].x) * (armor_list_[k] [0].y - armor_list_[k][2].y));
        if (area >= max_area)
        {
            max_area = area;
            armor_ = armor_list_[k];
        }
    }

    lamp_list.clear();
    point_list_.clear();
    area_list_.clear();
    angle_list_.clear();
    return armor_;
    //return true;
}